package application;

public class SPG4
{

	double X, Y, Z, XDOT, YDOT, ZDOT;
	Satellite sat;

	int IFLAG = 1; // markiert ersten Durchgang

	final double QO = 120.0; // parameter for the SGP4/SGP8 density function
	final double SO = 78.0; // parameter for the SGP4/SGP8 density function
	final double XJ2 = 1.082616E-3; // Gravitationsparameter: the second
	// gravitational zonal harmonic of the Earth
	final double XJ3 = -0.253881E-5; // Gravitationsparameter: the third
										// gravitational
	// zonal harmonic of the Earth
	final double XJ4 = -1.65597E-6;// Gravitationsparameter: the fourth
	// gravitational zonal harmonic of the Earth

	final double XKE = 0.743669161E-1;
	final double kmPerEarthRadius = 6378.135; // Kilometer pro Erdradius
	final double minPerDay = 1440.0; // Minuten pro Tag
	final double AE = 1.0; // Distanzeinheit: Erdradien
	final double CK2 = 0.5 * XJ2 * Math.pow(AE, 2); // Gravitationsparameter
	final double CK4 = -0.375 * XJ4 * Math.pow(AE, 4); // Gravitationsparameter
	final double QOMS2T = Math.pow(((QO - SO) * AE / kmPerEarthRadius), 4); // parameter
																			// for
																			// the
																			// SGP4/SGP8
																			// density
																			// function
	final double S = AE * (1.0 + SO / kmPerEarthRadius); // parameter for the
															// SGP4/SGP8 density
															// function

	// temp vars
	double XMDOT = 0;
	double OMGDOT = 0;
	double XNODOT = 0;
	double XNODCF = 0;
	double C1 = 0;
	double C4 = 0;
	double C5 = 0;
	double T2COF = 0;
	double OMGCOF = 0;
	double XMCOF = 0;
	double ETA = 0;
	double DELMO = 0;
	double D2 = 0;
	double D3 = 0;
	double D4 = 0;
	double SINMO = 0;
	double T3COF = 0;
	double T4COF = 0;
	double T5COF = 0;
	double AODP = 0;
	double XNODP = 0;
	double XLCOF = 0;
	double AYCOF = 0;
	double SINEPW = 0;
	double COSEPW = 0;
	double COSIO = 0;
	double SINIO = 0;
	double X3THM1 = 0;
	double X1MTH2 = 0;
	double X7THM1 = 0;
	double TEMP = 0;
	double TEMP1 = 0;
	double TEMP2 = 0;
	double TEMP3 = 0;
	double TEMP4 = 0;
	double TEMP5 = 0;
	double TEMP6 = 0;

	int ISIMP = 0;

	double XMDF;
	double OMGADF;
	double XNODDF;
	double OMEGA;
	double XMP;
	double TSQ;
	double XNODE;
	double TEMPA;
	double TEMPE;
	double TEMPL;

	// double EPOCH;
	// double DS50;
	double meanAnomaly;
	double rightAscension;
	double argumentOfPerigee;
	double eccentricity;
	double inclination;
	double meanMotion;
	double firstTimeDerivativeOfMeanMotion_divByTwo;
	double secondTimeDerivativeOfMeanMotion_divBySix;
	double bstarDragTerm;

	public SPG4(Satellite sat)
	{
		this.sat = sat;
		// this.EPOCH = sat.epochYear;
		// this.DS50 = sat.satelliteNumber;
		this.meanAnomaly = sat.meanAnomaly;
		this.rightAscension = sat.rightAscension;
		this.argumentOfPerigee = sat.argumentOfPerigee;
		this.eccentricity = sat.eccentricity;
		this.inclination = sat.inclination;
		this.meanMotion = sat.meanMotion;
		this.firstTimeDerivativeOfMeanMotion_divByTwo = sat.firstTimeDerivativeOfMeanMotion_divByTwo;
		this.secondTimeDerivativeOfMeanMotion_divBySix = sat.secondTimeDerivativeOfMeanMotion_divBySix;
		this.bstarDragTerm = sat.bstarDragTerm;

	}

	void calculateStep(double TSINCE)
	{

		// SEITE 16

		if (IFLAG != 0) // == 0: GO TO 100
		{

			// RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP)
			// FROM INPUT ELEMENTS
			double A1 = Math.pow((XKE / meanMotion), (2.0 / 3.0));
			COSIO = Math.cos(inclination);
			double THETA2 = COSIO * COSIO;
			X3THM1 = 3.0 * THETA2 - 1.0;
			double EOSQ = eccentricity * eccentricity;
			double BETAO2 = 1.0 - EOSQ;
			double BETAO = Math.sqrt(BETAO2);
			double DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
			double AO = A1 * (1.0 - DEL1 * (0.5 * (2.0 / 3.0) + DEL1 * (1.0 + 134.0 / 81.0 * DEL1)));
			double DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
			XNODP = meanMotion / (1.0 + DELO);
			AODP = AO / (1.0 - DELO);

			// INITIALIZATION
			// FOR PERIGEE LESS THAN 220 KILOMETERS, THE ISIMP FLAG IS SET AND
			// THE EQUATIONS ARE TRUNCATED TO LINEAR VARIATION IN SQRT A AND
			// QUADRATIC VARIATION IN MEAN ANOMALY. ALSO, THE C3 TERM, THE
			// / DELTA OMEGA TERM, AND THE DELTA M TERM ARE DROPPED.
			ISIMP = 0;
			if ((AODP * (1.0 - eccentricity) / AE) < (220.0 / kmPerEarthRadius + AE)) ISIMP = 1;
			// FOR PERIGEE BELOW 156 KM, THE VALUES OF
			// S AND QOMS2T ARE ALTERED
			double S4 = S;
			double QOMS24 = QOMS2T;
			double PERIGE = (AODP * (1.0 - eccentricity) - AE) * kmPerEarthRadius;
			if (!(PERIGE >= 156.0)) // if (PERIGE >= 156.0) GO TO 10;
			{
				S4 = PERIGE - 78.0;

				if (!(PERIGE > 98.0)) // if (PERIGE > 98.0) GO TO 9;
				{
					S4 = 20.0;
				}
				QOMS24 = Math.pow(((120.0 - S4) * AE / kmPerEarthRadius), 4); // 9

				S4 = S4 / kmPerEarthRadius + AE;
			}
			// SEITE 17
			double PINVSQ = 1.0 / (AODP * AODP * BETAO2 * BETAO2); // 10
			double TSI = 1.0 / (AODP - S4);
			ETA = AODP * eccentricity * TSI;
			double ETASQ = ETA * ETA;
			double EETA = eccentricity * ETA;
			double PSISQ = Math.abs(1.0 - ETASQ);
			double COEF = QOMS24 * Math.pow((TSI), 4.0);
			double COEF1 = COEF / Math.pow((PSISQ), 3.5);
			double C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ + EETA * (4.0 + ETASQ)) + 0.75 * CK2 * TSI / PSISQ * X3THM1 * (8.0 + 3.0 * ETASQ * (8.0 + ETASQ)));
			C1 = bstarDragTerm * C2;
			SINIO = Math.sin(inclination);
			double A3OVK2 = (-XJ3 / CK2) * Math.pow((AE), 3);
			double C3 = COEF * TSI * A3OVK2 * XNODP * AE * SINIO / eccentricity;
			X1MTH2 = 1.0 - THETA2;
			C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2
					* (ETA * (2.0 + 0.5 * ETASQ) + eccentricity * (0.5 + 2.0 * ETASQ) - 2.0 * CK2 * TSI / (AODP * PSISQ) * (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ * (1.5 - 0.5 * EETA)) + 0.75 * X1MTH2 * (2.0 * ETASQ - EETA * (1.0 + ETASQ)) * Math.cos(2.0 * argumentOfPerigee)));
			C5 = 2.0 * COEF1 * AODP * BETAO2 * (1.0 + 2.75 * (ETASQ + EETA) + EETA * ETASQ);
			double THETA4 = THETA2 * THETA2;
			TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
			TEMP2 = TEMP1 * CK2 * PINVSQ;
			TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
			XMDOT = XNODP + 0.5 * TEMP1 * BETAO * X3THM1 + 0.0625 * TEMP2 * BETAO * (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
			double X1M5TH = 1.0 - 5.0 * THETA2;
			OMGDOT = -0.5 * TEMP1 * X1M5TH + 0.0625 * TEMP2 * (7.0 - 114.0 * THETA2 + 395.0 * THETA4) + TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
			double XHDOT1 = -TEMP1 * COSIO;
			XNODOT = XHDOT1 + (0.5 * TEMP2 * (4.0 - 19.0 * THETA2) + 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
			OMGCOF = bstarDragTerm * C3 * Math.cos(argumentOfPerigee);
			XMCOF = -(2.0 / 3.0) * COEF * bstarDragTerm * AE / EETA;
			XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
			T2COF = 1.5 * C1;
			XLCOF = 0.125 * A3OVK2 * SINIO * (3.0 + 5.0 * COSIO) / (1.0 + COSIO);
			AYCOF = .25 * A3OVK2 * SINIO;
			DELMO = Math.pow((1.0 + ETA * Math.cos(meanAnomaly)), 3);
			SINMO = Math.sin(meanAnomaly);
			X7THM1 = 7.0 * THETA2 - 1.0;
			if (ISIMP != 1) // if (ISIMP == 1) GO TO 90;
			{
				double C1SQ = C1 * C1;
				D2 = 4.0 * AODP * TSI * C1SQ;
				TEMP = D2 * TSI * C1 / 3.0;
				D3 = (17.0 * AODP + S4) * TEMP;
				D4 = 0.5 * TEMP * AODP * TSI * (221.0 * AODP + 31.0 * S4) * C1;
				T3COF = D2 + 2.0 * C1SQ;
				T4COF = 0.25 * (3.0 * D3 + C1 * (12.0 * D2 + 10.0 * C1SQ));
				T5COF = 0.2 * (3.0 * D4 + 12.0 * C1 * D3 + 6.0 * D2 * D2 + 15.0 * C1SQ * (2.0 * D2 + C1SQ));
			}
			IFLAG = 0; // 90
		}
		// UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG
		XMDF = meanAnomaly + XMDOT * TSINCE; // 100
		OMGADF = argumentOfPerigee + OMGDOT * TSINCE;
		XNODDF = rightAscension + XNODOT * TSINCE;
		OMEGA = OMGADF;
		XMP = XMDF;
		TSQ = TSINCE * TSINCE;
		XNODE = XNODDF + XNODCF * TSQ;
		TEMPA = 1.0 - C1 * TSINCE;
		TEMPE = bstarDragTerm * C4 * TSINCE;
		TEMPL = T2COF * TSQ;

		if (ISIMP != 1) // IF(ISIMP .EQ. 1) GO TO 110;
		{
			double DELOMG = OMGCOF * TSINCE;
			double DELM = XMCOF * (Math.pow((1. + ETA * Math.cos(XMDF)), 3) - DELMO);
			TEMP = DELOMG + DELM;
			XMP = XMDF + TEMP;
			OMEGA = OMGADF - TEMP;
			double TCUBE = TSQ * TSINCE;
			double TFOUR = TSINCE * TCUBE;
			TEMPA = TEMPA - D2 * TSQ - D3 * TCUBE - D4 * TFOUR;
			TEMPE = TEMPE + bstarDragTerm * C5 * (Math.sin(XMP) - SINMO);
			TEMPL = TEMPL + T3COF * TCUBE + TFOUR * (T4COF + TSINCE * T5COF);
		}
		double A = AODP * Math.pow(TEMPA, 2); // 110
		double E = eccentricity - TEMPE;
		double XL = XMP + OMEGA + XNODE + XNODP * TEMPL;
		double BETA = Math.sqrt(1.0 - E * E);
		double XN = XKE / Math.pow(A, 1.5);

		// LONG PERIOD PERIODICS
		double AXN = E * Math.cos(OMEGA);
		TEMP = 1.0 / (A * BETA * BETA);
		double XLL = TEMP * XLCOF * AXN;
		double AYNL = TEMP * AYCOF;
		double XLT = XL + XLL;
		double AYN = E * Math.sin(OMEGA) + AYNL;

		// SOLVE KEPLERS EQUATION
		double CAPU = FMOD2P(XLT - XNODE);

		// SEITE 18
		TEMP2 = CAPU;
		for (int I = 1; I <= 10; I++) // DO 130 I=1,10
		{
			SINEPW = Math.sin(TEMP2);
			COSEPW = Math.cos(TEMP2);
			TEMP3 = AXN * SINEPW;
			TEMP4 = AYN * COSEPW;
			TEMP5 = AXN * COSEPW;
			TEMP6 = AYN * SINEPW;
			double EPW = (CAPU - TEMP4 + TEMP3 - TEMP2) / (1.0 - TEMP5 - TEMP6) + TEMP2;
			if (Math.abs(EPW - TEMP2) <= 1.0E-6) I = 11;// GO TO 140;
			TEMP2 = EPW;
		} // END DO 130

		// SHORT PERIOD PRELIMINARY QUANTITIES
		double ECOSE = TEMP5 + TEMP6; // 140
		double ESINE = TEMP3 - TEMP4;
		double ELSQ = AXN * AXN + AYN * AYN;
		TEMP = 1.0 - ELSQ;
		double PL = A * TEMP;
		double R = A * (1.0 - ECOSE);
		TEMP1 = 1.0 / R;
		double RDOT = XKE * Math.sqrt(A) * ESINE * TEMP1;
		double RFDOT = XKE * Math.sqrt(PL) * TEMP1;
		TEMP2 = A * TEMP1;
		double BETAL = Math.sqrt(TEMP);
		TEMP3 = 1.0 / (1.0 + BETAL);
		double COSU = TEMP2 * (COSEPW - AXN + AYN * ESINE * TEMP3);
		double SINU = TEMP2 * (SINEPW - AYN - AXN * ESINE * TEMP3);
		// double U = ACTAN(SINU, COSU);
		double U = Math.atan2(SINU, COSU);
		double SIN2U = 2.0 * SINU * COSU;
		double COS2U = 2.0 * COSU * COSU - 1.0;
		TEMP = 1.0 / PL;
		TEMP1 = CK2 * TEMP;
		TEMP2 = TEMP1 * TEMP;
		// UPDATE FOR SHORT PERIODICS
		double RK = R * (1.0 - 1.5 * TEMP2 * BETAL * X3THM1) + 0.5 * TEMP1 * X1MTH2 * COS2U;
		double UK = U - 0.25 * TEMP2 * X7THM1 * SIN2U;
		double XNODEK = XNODE + 1.5 * TEMP2 * COSIO * SIN2U;
		double XINCK = inclination + 1.5 * TEMP2 * COSIO * SINIO * COS2U;
		double RDOTK = RDOT - XN * TEMP1 * X1MTH2 * SIN2U;
		double RFDOTK = RFDOT + XN * TEMP1 * (X1MTH2 * COS2U + 1.5 * X3THM1);
		// ORIENTATION VECTORS
		double SINUK = Math.sin(UK);
		double COSUK = Math.cos(UK);

		// SEITE 19
		double SINIK = Math.sin(XINCK);
		double COSIK = Math.cos(XINCK);
		double SINNOK = Math.sin(XNODEK);
		double COSNOK = Math.cos(XNODEK);
		double XMX = -SINNOK * COSIK;
		double XMY = COSNOK * COSIK;
		double UX = XMX * SINUK + COSNOK * COSUK;
		double UY = XMY * SINUK + SINNOK * COSUK;
		double UZ = SINIK * SINUK;
		double VX = XMX * COSUK - COSNOK * SINUK;
		double VY = XMY * COSUK - SINNOK * SINUK;
		double VZ = SINIK * COSUK;
		// POSITION AND VELOCITY
		X = RK * UX;
		Y = RK * UY;
		Z = RK * UZ;
		XDOT = RDOTK * UX + RFDOTK * VX;
		YDOT = RDOTK * UY + RFDOTK * VY;
		ZDOT = RDOTK * UZ + RFDOTK * VZ;

		sat.x = X;
		sat.y = Y;
		sat.z = Z;
		sat.xDot = XDOT;
		sat.yDot = YDOT;
		sat.zDot = ZDOT;

	}

	double _FMOD2P(double X)
	{
		double FMOD2P = X;
		double I = FMOD2P / (2 * Math.PI);
		FMOD2P = FMOD2P - I * (2 * Math.PI);
		if (FMOD2P < 0) FMOD2P = FMOD2P + (2 * Math.PI);
		return X;
	}

	protected static double FMOD2P(final double testValue)
	{
		/* Returns mod 2PI of argument */

		double retVal = testValue;
		final int i = (int) (retVal / (2 * Math.PI));
		retVal -= i * (2 * Math.PI);

		if (retVal < 0.0)
		{
			retVal += (2 * Math.PI);
		}

		return retVal;
	}

	double ACTAN(double SINX, double COSX)
	{
		int GOTO = 0;
		double ACTAN = 0.0;

		while (true)
		{

			if (COSX == 0.0) GOTO = 5;
			if (COSX > 0.0) GOTO = 1;
			if (GOTO == 0)
			{
				ACTAN = Math.PI;
				GOTO = 7;
			}
			if (GOTO == 1)
			{
				if (SINX == 0.0) GOTO = 8;
				if (SINX > 0.0) GOTO = 7;
				if (GOTO == 1)
				{
					ACTAN = 2 * Math.PI;
					GOTO = 7;
				}
			}
			if (GOTO == 5)
			{
				if (SINX == 0.0) GOTO = 8;
				if (SINX > 0.0) GOTO = 6;
				if (GOTO == 5)
				{
					ACTAN = 3 * Math.PI / 2;
					GOTO = 8;
				}
			}
			if (GOTO == 6)
			{
				ACTAN = Math.PI / 2;
				GOTO = 8;
			}

			if (GOTO == 7)
			{
				double TEMP = SINX / COSX;
				ACTAN = ACTAN + Math.atan(TEMP);
				GOTO = 8;
			}

			if (GOTO == 8) return ACTAN;
		}
	}
}
